#ifndef TRANSFORMS_H
#define TRANSFORMS_H

#include <matrix4x4.h>
#include <vector3.h>
#include <coord.h>
#include <math.h>
#include <stdlib.h>

matrix4x4_t *
compile_pipeline( matrix4x4_t *matrices[] );

void
free_pipeline_matrices( matrix4x4_t *matrices[] );


static inline void
multiply_matrix4x4_vector3( const matrix4x4_t m, const vector3_t v,
			    vector3_t *r )
{
  r->data[ 0 ] = m.data[ 0 ][ 0 ] * v.data[ 0 ] + \
                 m.data[ 0 ][ 1 ] * v.data[ 1 ] + \
                 m.data[ 0 ][ 2 ] * v.data[ 2 ] + \
                 m.data[ 0 ][ 3 ];

  r->data[ 1 ] = m.data[ 1 ][ 0 ] * v.data[ 0 ] + \
                 m.data[ 1 ][ 1 ] * v.data[ 1 ] + \
                 m.data[ 1 ][ 2 ] * v.data[ 2 ] + \
                 m.data[ 1 ][ 3 ];

  r->data[ 2 ] = m.data[ 2 ][ 0 ] * v.data[ 0 ] + \
                 m.data[ 2 ][ 1 ] * v.data[ 1 ] + \
                 m.data[ 2 ][ 2 ] * v.data[ 2 ] + \
                 m.data[ 2 ][ 3 ];
}

static inline void
multiply_matrix4x4_coord3d( const matrix4x4_t m, const coord3d_t c,
			    coord3d_t *r )
{
  r->x = (uint16_t)( m.data[ 0 ][ 0 ] * c.x + \
		     m.data[ 0 ][ 1 ] * c.y + \
		     m.data[ 0 ][ 2 ] * c.z + \
		     m.data[ 0 ][ 3 ] );

  r->y = (uint16_t)( m.data[ 1 ][ 0 ] * c.x + \
		     m.data[ 1 ][ 1 ] * c.y + \
		     m.data[ 1 ][ 2 ] * c.z + \
		     m.data[ 1 ][ 3 ] );

  r->z = (uint16_t)( m.data[ 2 ][ 0 ] * c.x + \
		     m.data[ 2 ][ 1 ] * c.y + \
		     m.data[ 2 ][ 2 ] * c.z + \
		     m.data[ 2 ][ 3 ] );
}


static inline matrix4x4_t *
rotate_z( const double radians )
{
  double s = sin( radians );
  double c = cos( radians );
  matrix4x4_t *m = matrix4x4_new_identity();

  m->data[ 0 ][ 0 ] =   c;
  m->data[ 0 ][ 1 ] = - s;

  m->data[ 1 ][ 0 ] =   s;
  m->data[ 1 ][ 1 ] =   c;

  return m;
}

static inline matrix4x4_t *
rotate_x( const double radians )
{
  double s = sin( radians );
  double c = cos( radians );
  matrix4x4_t *m = matrix4x4_new_identity();

  m->data[ 1 ][ 1 ] =   c;
  m->data[ 1 ][ 2 ] = - s;

  m->data[ 2 ][ 1 ] =   s;
  m->data[ 2 ][ 2 ] =   c;

  return m;
}

static inline matrix4x4_t *
rotate_y( const double radians )
{
  double s = sin( radians );
  double c = cos( radians );
  matrix4x4_t *m = matrix4x4_new_identity();

  m->data[ 0 ][ 0 ] =   c;
  m->data[ 0 ][ 2 ] =   s;

  m->data[ 2 ][ 0 ] = - s;
  m->data[ 2 ][ 2 ] =   c;

  return m;
}

static inline matrix4x4_t *
translate( const vector3_t v )
{
  matrix4x4_t *m = matrix4x4_new_identity();

  m->data[ 0 ][ 3 ] = v.data[ 0 ];
  m->data[ 1 ][ 3 ] = v.data[ 1 ];
  m->data[ 2 ][ 3 ] = v.data[ 2 ];

  return m;
}

static inline matrix4x4_t *
inv_translate( const vector3_t v )
{
  matrix4x4_t *m = matrix4x4_new_identity();

  m->data[ 0 ][ 3 ] = - v.data[ 0 ];
  m->data[ 1 ][ 3 ] = - v.data[ 1 ];
  m->data[ 2 ][ 3 ] = - v.data[ 2 ];

  return m;
}


static inline matrix4x4_t *
scale( const vector3_t v )
{
  matrix4x4_t *m = matrix4x4_new();

  m->data[ 0 ][ 0 ] = v.data[ 0 ];
  m->data[ 1 ][ 1 ] = v.data[ 1 ];
  m->data[ 2 ][ 2 ] = v.data[ 2 ];
  m->data[ 3 ][ 3 ] = 1;

  return m;
}

static inline matrix4x4_t *
inv_scale( const vector3_t v )
{
  matrix4x4_t *m = matrix4x4_new();

  m->data[ 0 ][ 0 ] = 1.0 / v.data[ 0 ];
  m->data[ 1 ][ 1 ] = 1.0 / v.data[ 1 ];
  m->data[ 2 ][ 2 ] = 1.0 / v.data[ 2 ];
  m->data[ 3 ][ 3 ] = 1;

  return m;
}

static inline matrix4x4_t *
rotate_around_point( matrix4x4_t *rotate, const vector3_t point )
{
  matrix4x4_t *m;

  matrix4x4_t *pipeline[] = {
    inv_translate( point ),
    rotate,
    translate( point ),
    NULL,
  };

  m = compile_pipeline( pipeline );

  matrix4x4_free( pipeline[ 0 ] );
  matrix4x4_free( pipeline[ 2 ] );

  return m;
}



#endif /* TRANSFORMS_H */
